Note: This is based on the image_transport Tutorials Publishing Images and Subscribing to Images.
(!) Please ask about problems and questions regarding this tutorial on answers.ros.org. Don't forget to include in your question the link to this page, the versions of your OS & ROS, and also add appropriate tags.

Writing a simple image processor

Description: This code snippet shows how to modify and create a sensor_msgs/Image.

Keywords: sensor_msgs/Image image shrinker

Tutorial Level: BEGINNER

Explanation

I nowhere found a help on using sensor_msgs/Image, so this is my example on manipulating an Image in image_transport.

The image_shrinker receives an Image, scales it down in a most primitive way, and publishes it. It's intended for remote video monitoring using less bandwidth.

Code-Snippet

   1 #include <ros/ros.h>
   2 #include <image_transport/image_transport.h>
   3 
   4 #define DIVISOR 4
   5 
   6 image_transport::Publisher pub;
   7 
   8 void imageCallback(const sensor_msgs::ImageConstPtr& old_image)
   9 {
  10         // created shared pointer Image
  11         sensor_msgs::Image::Ptr small_image =
  12                         boost::make_shared<sensor_msgs::Image>();
  13 
  14         // copy image properties
  15         small_image->header = old_image->header;
  16         small_image->height = old_image->height / DIVISOR;
  17         small_image->width = old_image->width / DIVISOR;
  18         small_image->encoding = old_image->encoding;
  19         small_image->is_bigendian = old_image->is_bigendian;
  20         small_image->step = old_image->step / DIVISOR;
  21 
  22         small_image->data.resize(small_image->width * small_image->height);
  23 
  24         // copy every DIVISORth byte
  25         // subpixels will be merged if multiple bytes per pixel
  26         uint new_index = 0;
  27         for(uint row = 0; row < small_image->height; row++) {
  28                 int row_offset = row*old_image->step*DIVISOR;
  29                 for(uint col = 0; col < small_image->width; col++) {
  30                         int old_index = row_offset + col*DIVISOR;
  31                         small_image->data[new_index++] =
  32                                         old_image->data[old_index];
  33                 }
  34         }
  35 
  36         pub.publish(small_image);
  37 }
  38 
  39 int main(int argc, char **argv)
  40 {
  41         ros::init(argc, argv, "image_shrinker");
  42         ros::NodeHandle nh;
  43         image_transport::ImageTransport it(nh);
  44         image_transport::Subscriber sub =
  45                         it.subscribe("camera/image_raw", 1, imageCallback);
  46         pub = it.advertise("camera/image_small", 1);
  47 
  48         ros::spin();
  49         printf("Im out! No error.");
  50 }

Execution

Save the code to a package, e.g. learning_image_transport from above mentioned tutorials, and add following to the CMakeLists.txt:

rosbuild_add_executable(image_shrinker src/image_shrinker.cpp)

Wiki: image_pipeline/Tutorials/Simple image processor (last edited 2011-07-06 11:48:27 by FelixKolbe)